#include "lh_ecg.h"


void lh_ecg_init(void)
{
    LH3001_ADC_INIT_t adc_init;
    LH3001_PGA_INIT_t pga_init;
    LH3001_LEADOFF_t leadoff_init;


    lh3001_adc_stop();


#if 1
    leadoff_init.threshold = LH3001_LEADOFF_DELTA_10;
    leadoff_init.current = LH3001_LEADOFF_CURRENT_SOURCE_25nA;
    leadoff_init.sink_cfg = LH3001_LEADOFF_SINK_AIN0_1;
    leadoff_init.source_cfg = LH3001_LEADOFF_SOURCE_CLOSE;
    leadoff_init.rld_sink_en = 1;
    leadoff_init.ain0_detect_en = 1;
    leadoff_init.ain1_detect_en = 1;
    leadoff_init.rld_detect_en = 1;
    lh3001_leadoff_init(&leadoff_init);
#endif


    lh3001_rld_init(1,ENUM_LH3001_RLD_REF_AVDD_DIV2,ENUM_LH3001_RLD_CHOP_FMOD_DIV32);

    pga_init.gain = ENUM_LH3001_PGA_PGAGAIN_6;
    pga_init.pga_bypass = 0;
    pga_init.power_mode = ENUM_LH3001_PGA_POWER_LP;
    pga_init.comm_sense_en = 1;
    pga_init.pga_chop = ENUM_LH3001_PGA_CHOP_DIV16;
    lh3001_pga_init(&pga_init);

    adc_init.Fmodclk = ENUM_LH3001_ADC_FMODCLK_128K;
    adc_init.oversample_rate = ENUM_LH3001_ADC_DR_DIV512; ; //update rate = Fmodclk/oversample_rate
    adc_init.conv_mode = ENUM_LH3001_ADC_CONTINOUS;
    adc_init.ref_sel = ENUM_LH3001_ADC_REF_2V5;
    lh3001_adc_init(&adc_init);
    //lh3001_adc_channel(ENUM_LH3001_ADCCHP_VCM,ENUM_LH3001_ADCCHN_GND);	//FOR NOISE TEST
	lh3001_adc_channel(ENUM_LH3001_ADCCHP_AIN0,ENUM_LH3001_ADCCHN_AIN1);
	
    lms_filter_init();
	panTompkinsInit();

}



void lh_ecg_start(void)
{
   lh3001_adc_go();
   lh3001_rdatac_start(); 
}

void lh_ecg_stop(void)
{
   lh3001_adc_stop();
   lh3001_rdatac_stop(); 
}



#define CHEBV_FILTER_FACTOR_LEN  5
const float b1[]={
    0.6909726,  -0.85406234,  1.62608257, -0.85406234,  0.6909726
};
const float a1[]={
    -1.01590208,  1.52762246, -0.69222259,  0.4804053, 0
};
const float b2[]={
    0.6909726,  2.23596422, 3.18337557, 2.23596422, 0.6909726
};
const float a2[]={
    2.65966618, 3.08491546, 1.81226227, 0.4804053, 0
};


/*
    Fs = 250Hz
    4-order chebv band-stop filter at frequency [45Hz,55Hz] and [90Hz,110Hz]
*/
static float xn1[CHEBV_FILTER_FACTOR_LEN] = {0};
static float yn1[CHEBV_FILTER_FACTOR_LEN] = {0};
static float xn2[CHEBV_FILTER_FACTOR_LEN] = {0};
static float yn2[CHEBV_FILTER_FACTOR_LEN] = {0};
float lh_filter_50Hz(float input)
{
    float sum = 0;
    float input2 = 0;
    float sum2 = 0;

    sum += b1[0] * input;
    for(uint32_t i=CHEBV_FILTER_FACTOR_LEN-1; i > 0; i--)
    {
        sum += b1[i] * xn1[i-1];
        xn1[i] = xn1[i-1];
        sum -= a1[i] * yn1[i];
        yn1[i] = yn1[i-1];
    }
    sum -= a1[0]*yn1[0];
    yn1[0] = sum;
    xn1[0] = input;

    input2 = sum;
    sum2 += b2[0] * sum;
    for(uint32_t i=CHEBV_FILTER_FACTOR_LEN-1; i > 0; i--)
    {
        sum2 += b2[i] * xn2[i-1];
        xn2[i] = xn2[i-1];
        sum2 -= a2[i] * yn2[i];
        yn2[i] = yn2[i-1];
    }
    sum2 -= a2[0]*yn2[0];
    yn2[0] = sum2;
    xn2[0] = input2;

    return sum2;
}


const float hp_b[] = {0.98132818, -1.96257889,  0.98132818};
const float hp_a[] = {-1.96223021,  0.96300504,0};
static float hp_xn1[3] = {0};
static float hp_yn1[3] = {0};
float lh_filter_highpass(float input)
{
    float sum = 0;

    sum += hp_b[0] * input;
    for(uint32_t i=3-1; i > 0; i--)
    {
        sum += hp_b[i] * hp_xn1[i-1];
        hp_xn1[i] = hp_xn1[i-1];
        sum -= hp_a[i] * hp_yn1[i];
        hp_yn1[i] = hp_yn1[i-1];
    }
    sum -= hp_a[0]*hp_yn1[0];
    hp_yn1[0] = sum;
    hp_xn1[0] = input;

    return sum;
}


const float lp_b[7] = {
  0.02297348, -0.0055155,   0.03615661,  0.00371042,  0.03615661, -0.0055155,
  0.02297348,
};
const float lp_a[7] = {
 -2.85638443,  3.90728836, -3.01642343,  1.38819349, -0.3507606,
  0.0390262, 0
};
 
static float lp_xn1[7] = {0};
static float lp_yn1[7] = {0};
float lh_filter_lowpass(float input)
{
    float sum = 0;

    sum += lp_b[0] * input;
    for(uint32_t i=7-1; i > 0; i--)
    {
        sum += lp_b[i] * lp_xn1[i-1];
        lp_xn1[i] = lp_xn1[i-1];
        sum -= lp_a[i] * lp_yn1[i];
        lp_yn1[i] = lp_yn1[i-1];
    }
    sum -= lp_a[0]*lp_yn1[0];
    lp_yn1[0] = sum;
    lp_xn1[0] = input;

    return sum;
}

const float nortch_b[3] = {
  0.98712946, -0.9942826,   0.98712946,
};
const float nortch_a[3] = {
  -0.9942826,   0.97425892,0
};

static float notch_xn1[3] = {0};
static float notch_yn1[3] = {0};
float lh_filter_50notch(float input)
{
    float sum = 0;

    sum += nortch_b[0] * input;
    for(uint32_t i=3-1; i > 0; i--)
    {
        sum += nortch_b[i] * notch_xn1[i-1];
        notch_xn1[i] = notch_xn1[i-1];
        sum -= nortch_a[i] * notch_yn1[i];
        notch_yn1[i] = notch_yn1[i-1];
    }
    sum -= nortch_a[0]*notch_yn1[0];
    notch_yn1[0] = sum;
    notch_xn1[0] = input;

    return sum;
}




